//Code written by Rachit
#include "linefollower.h"
#include <Arduino.h>


LineFollower::LineFollower()
{

}

void LineFollower::readSensor()                                       // function to read from sensors
{
  lsensor3val=digitalRead(lsensor3);                                   // lsensor3val is public variable of class LineFollower
  lsensor2val=digitalRead(lsensor2);
  lsensor1val=digitalRead(lsensor1);
  csensorval=digitalRead(csensor);
  rsensor1val=digitalRead(rsensor1);
  rsensor2val=digitalRead(rsensor2);
  rsensor3val=digitalRead(rsensor3);
}
int LineFollower::sensorVal(int SensorNum)
{
  switch(SensorNum)
  {
    case 0:  return csensorval;break;
    case 1:  return rsensor1val;break;
    case 2:  return rsensor2val;break;
    case 3:  return rsensor3val;break;
    case -1:  return lsensor1val;break;
    case -2:  return lsensor2val;break;
    case -3:  return lsensor3val;break;
  }
}

void LineFollower::setK(double kp,double ki,double kd)
{
  LineFollower::Kp=kp;                                                        //setting the costant of PID to class
  LineFollower::Ki=ki;
  LineFollower::Kd=kd;
}
void LineFollower::flushError()
{
  dt=0;	//Initial time of the code
  error=0;
  deviation=0;
  totalerror=0;
  prevdeviation=0;
  prevTime=0;
  devcount=0;
}
int LineFollower::calculateError()
{
  
  dt=millis()-prevTime;
  prevTime=millis();
  
  int noofsensors;
  noofsensors=lsensor3val + lsensor2val + lsensor1val + csensorval  + rsensor1val + rsensor2val + rsensor3val;
  if(noofsensors<4)
  {
    	int a;
		a=2*2*2*2*2*2*lsensor3val + 2*2*2*2*2*lsensor2val + 2*2*2*2*lsensor1val + 2*2*2*csensorval  + 2*2*rsensor1val + 2*rsensor2val + rsensor3val;
		switch(ERROR_DISTRIBUTION)
		{
						
			case 'P':
			case 'p':	
					    Serial.print(a,BIN);
						Serial.print(" dt:");	Serial.print(dt);
						if(a==0b01000000) deviation=-11;
					    if(a==0b01100000)deviation=-8;
					    if(a==0b00100000)deviation=-6;
					    if(a==0b00110000)deviation=-4;
					    if(a==0b00010000)deviation=-2;
					    if(a==0b00011000)deviation=-1;
					    if(a==0b00001000)deviation=0;
					    if(a==0b00001100)deviation=1;
					    if(a==0b00000100)deviation=2;
					    if(a==0b00000110)deviation=4;
					    if(a==0b00000010)deviation=6;
   					    if(a==0b00000011)deviation=8;
					    if(a==0b00000001)deviation=11;
					    if(deviation < -11 || deviation > 11) return 0;
							
						Serial.print("   Deviation:");	Serial.print(deviation);
						break;
			case 'L':
			case 'l':	
						if(a==0b01000000) deviation=-6;
						if(a==0b01100000)deviation=-5;
						if(a==0b00100000)deviation=-4;
						if(a==0b00110000)deviation=-3;
						if(a==0b00010000)deviation=-2;
						if(a==0b00011000)deviation=-1;
						if(a==0b00001000)deviation=0;
						if(a==0b00001100)deviation=1;
						if(a==0b00000100)deviation=2;
						if(a==0b00000110)deviation=3;
						if(a==0b00000010)deviation=4;
						if(a==0b00000011)deviation=5;
						if(a==0b00000001)deviation=6;
						if(deviation < -6 || deviation > 6) return 0;
							break;
			case 'W':
			case 'w':	
				if(noofsensors==0 && (deviation<-3 || deviation>3))
				{  if(deviation>0)
					{
						deviation=WEIGHT4;
					}
				   else
					{
						deviation=-1*WEIGHT4;
					}
				}
				else
				{
					deviation=((-1*WEIGHT3*lsensor3val) + (-1*WEIGHT2*lsensor2val) + (-1*WEIGHT1*lsensor1val) + (0*csensorval) + (WEIGHT1*rsensor1val) + (WEIGHT2*rsensor2val) + (WEIGHT3*rsensor3val));
					if(noofsensors)
						deviation=deviation/noofsensors;break;
				}
		}
	}

  switch(dt_PID)	//Whether to implement dt or not
  {
	case 'y' :
	case 'Y' :
				sumprevdev[devcount]=(deviation-prevdeviation)/dt;
				totalerror+=error*dt;
				prevdeviation=deviation;	break;
	case 'n' :
	case 'N' :
				sumprevdev[devcount]=(deviation-prevdeviation);
				totalerror+=error;
				prevdeviation=deviation;	break;				
  }
  error=Kp*deviation+Ki*totalerror+Kd*(sumprevdev[0]+sumprevdev[1]+sumprevdev[2]+sumprevdev[3]+sumprevdev[4]);
  if(devcount>=4)
  {
    devcount=0;
  }
  else
  {devcount++;
  }
}

void LineFollower::followLine()
{
  LineFollower::readSensor();
  LineFollower::calculateError();
  int FLSpeed; //the current pwm of FL motor
  int FRSpeed; // "    "        "       "
  int BLSpeed; //  "      "      "  "
  int BRSpeed; // "      "     "      "
  //Both the functions below perform the same task, therefore the bot can turn in only one direction--edited by sumit


    if(isMovingForward())
    {
      FLSpeed=100+error;		//turn right of forward  //= meanspeed+error
      BLSpeed=100+error;
      FRSpeed=100-error;
      BRSpeed=100-error;
      Base::setPwm(FLSpeed,FRSpeed,BLSpeed,BRSpeed);
	  Serial.print("   FLspeed=         ");Serial.print(FLSpeed);
    Serial.print("   FRspeed=       ");Serial.println(FRSpeed);  
	
    }

    else if(isMovingBackward())
    {
      FLSpeed=100+error;
      BLSpeed=100+error;
      FRSpeed=100-error;
      BRSpeed=100-error;			//turn right of backward move
      Base::setPwm(FLSpeed,FRSpeed,BLSpeed,BRSpeed);
    }

  }

void LineFollower::attachSensor(int lsensor3,int lsensor2,int lsensor1,int csensor,int rsensor1,int rsensor2,int rsensor3)
{
  LineFollower::lsensor3=lsensor3;
  LineFollower::lsensor2=lsensor2;
  LineFollower::lsensor1=lsensor1;
  LineFollower::csensor=csensor;
  LineFollower::rsensor1=rsensor1;
  LineFollower::rsensor2=rsensor2;
  LineFollower::rsensor3=rsensor3;
}

void LineFollower::printSensor()
{
  Serial.print(lsensor3val);
  Serial.print(lsensor2val);
  Serial.print(lsensor1val);
  Serial.print(csensorval);
  Serial.print(rsensor1val);
  Serial.print(rsensor2val);
  Serial.println(rsensor3val);
}

